
#include "PathFindBehaviour.h"
#include "CommonBehaviours.h"
#include "RRT.h"
#include "../World.h"
#include "../Map.h"
#include "../WorldObject.h"

#include <cassert>
#include <cmath>
#include <GL/gl.h>
#include <vector>

PathRender::PathRender(std::vector<Vector2D> path) :
    path(path), is_active(true) {

}

PathRender::~PathRender(){

}
 
void PathRender::render(void){
    glColor3f(0.0f, 1.0f, 0.0f);

    glBegin(GL_LINE_STRIP);
    for(unsigned i = 0; i < path.size(); i++){
        glVertex3f(path[i].x, path[i].y, 0.0f);
    }
    glEnd();

    glColor3f(1.0f, 0.0f, 0.0f);
    glBegin(GL_POINTS);
    for(unsigned i = 0; i < path.size(); i++){
        glVertex3f(path[i].x, path[i].y, 0.0f);
    }
    glEnd();

}

void PathRender::update(void){
}

bool PathRender::active(void){
    return is_active;
}

bool PathRender::resolveCollision(Ship *ship){ return false; }
bool PathRender::resolveCollision(Weapon *weapon){ return false; }
bool PathRender::resolveCollision(Map *map){ return false; }

void PathRender::kill(void){ is_active = false; }


PathFindBehaviour::PathFindBehaviour(Ship *my_ship, Vector2D target) :
    ship(my_ship), target_point(target) {

    assert(my_ship != NULL);
    
    RRT rrt(World::instance().getMap(), 256);
    std::vector<Vector2D> tmp_path = rrt.planPath(ship->getPosition(), target_point);

    if(tmp_path.size() == 0){
        cur_waypoint = 0;
        std::cerr << "Error in PathFindBehaviour, path is of size 0" << std::endl;
    }
    else{
        waypoints = tmp_path;
        cur_waypoint = 1;
/*
        for(unsigned i = 0; i < tmp_path.size()-1; i++){
            waypoints.push_back(tmp_path[i]);
            for(unsigned j = 1; j < 10; j++){
                waypoints.push_back((j/10.0f)*tmp_path[i+1] + (1.0f - j/10.0f)*tmp_path[i]);
            }
        }
        waypoints.push_back(tmp_path.back());*/
    }

    //my_render = new PathRender(waypoints);
    //World::instance().addObject(my_render);
}

PathFindBehaviour::~PathFindBehaviour(){
    //my_render->kill();
}

bool PathFindBehaviour::goToTarget(void){
    if(cur_waypoint >= waypoints.size()){ return true; }

    while(cur_waypoint < waypoints.size()-1 && 
          CommonBehaviours::canSeePoint(ship, waypoints[cur_waypoint+1], true, false)){
        cur_waypoint++;
    }

    if(CommonBehaviours::goDirectToPoint(ship, waypoints[cur_waypoint])){
        cur_waypoint++;
    }

    if(cur_waypoint >= waypoints.size()){ return true; }
    return false;
}

Vector2D PathFindBehaviour::getTargetPos(void){
    if(waypoints.size() == 0){ return Vector2D(0.0f, 0.0f); }
    return waypoints.back();
}

bool PathFindBehaviour::make_valid(void){
    if(cur_waypoint >= waypoints.size()){ 
        return false; 
    }

    unsigned iter = 0;
    while(cur_waypoint > 0 && iter < 3){
        if(CommonBehaviours::canSeePoint(ship, waypoints[cur_waypoint], true, true)){
            return true;
        }
        cur_waypoint--;
        iter++;
    }

    return false;
}

